//
// GPSMapEdit
// (c) Konstantin Galichsky (kg@geopainting.com), 2002-2007
// (c) Philipp Peshin (phil@mobilae.ru), 2003
//
//  Basic geometric algorithms for points and rectangles.
//

# include "StdAfx.h"
# include "PtAlgo.h"
# include "fast_floor.h"
# include "Globals.h"

void aff_transform_t::ApplyTo (point_t & _p) const {
	const float x = _p.x, y = _p.y;

	_p.x = fAXx*x + fAXy*y + fBX;
	_p.y = fAYx*x + fAYy*y + fBY;
}

void aff_transform_t::Multiply (const aff_transform_t & _at2, aff_transform_t & _result) const {
	assert (& _result != this);

	_result.fAXx = fAXx*_at2.fAXx + fAXy*_at2.fAYx;
	_result.fAXy = fAXx*_at2.fAXy + fAXy*_at2.fAYy;
	_result.fAYx = fAYx*_at2.fAXx + fAYy*_at2.fAYx;
	_result.fAYy = fAYx*_at2.fAXy + fAYy*_at2.fAYy;

	_result.fBX = fBX + fAXx*_at2.fBX + fAXy*_at2.fBY;
	_result.fBY = fBY + fAYx*_at2.fBX + fAYy*_at2.fBY;
}

void aff_transform_t::Reverse (aff_transform_t & _result) const {
	assert (& _result != this);

	const float D = fAXx*fAYy - fAXy*fAYx;
	_result.fAXx = fAYy/D;
	_result.fAXy = -fAXy/D;
	_result.fAYx = -fAYx/D;
	_result.fAYy = fAXx/D;

	_result.fBX = - (_result.fAXx*fBX + _result.fAXy*fBY);
	_result.fBY = - (_result.fAYx*fBX + _result.fAYy*fBY);
}

void q_transform_t::ApplyTo (point_t & _p) const {
	const float x = _p.x, y = _p.y;

	_p.x = fDXxx*x*x + fDXxy*x*y + fDXyy*y*y + fAXx*x + fAXy*y + fBX;
	_p.y = fDYxx*x*x + fDYxy*x*y + fDYyy*y*y + fAYx*x + fAYy*y + fBY;
}

//////////////////////////////////////////////////////////

bool IsRightOriented (const points_t & _points) {
	float fArea2 = 0;

	const size_t cPoints = _points.size ();
	for (size_t cPoint = 0; cPoint < cPoints; ++ cPoint) {
		const point_t & p0 = _points [cPoint];
		const point_t & p1 = _points [cPoint + 1 == cPoints ? 0 : cPoint + 1];

		fArea2 += (p1.x - p0.x)*(p0.y + p1.y);
	}

	return fArea2 > 0;
}

float GetArea (const points_t & _points) {
	const size_t cPoints = _points.size ();
	if (cPoints <= 2)
		return 0;

	double fArea = 0;
	for (size_t cPoint = 0; cPoint < cPoints; ++ cPoint) {
		const point_t & p0 = _points [cPoint];
		const point_t & p1 = _points [cPoint + 1 == cPoints ? 0 : cPoint + 1];

		fArea += (p1.x - p0.x)*(p0.y + p1.y);
	}

	return ::fabs (fArea)*.5;
}

float GetLength (float _x1, float _y1, float _x2, float _y2) {
	const float fCosY = ::cos (_y1*c_PI/180.f);
	const float fCos2Y = fCosY*fCosY;

	const float dx = _x1 - _x2;
	const float dy = _y1 - _y2;
	return ::sqrt (dx*dx*fCos2Y + dy*dy);
}

float GetLength (const point_t & _p1, const point_t & _p2) {
	return GetLength (_p1.x, _p1.y, _p2.x, _p2.y);
}

float GetLength (const points_t & _points) {
	return GetLength (_points, 0, _points.size ());
}

float GetLength (const points_t & _points, size_t _cBegin, size_t _cEnd) {
	const size_t cPoints = _points.size ();
	if (cPoints <= 1)
		return 0;

	if (_cEnd > cPoints)
		_cEnd = cPoints;

	const double fCosY = ::cos (_points [0].y*c_PI/180.f);
	const double fCosY2 = fCosY*fCosY;

	double fLength = 0;
	for (size_t cPoint = _cBegin; cPoint + 1 < _cEnd; ++ cPoint) {
		const point_t & p0 = _points [cPoint];
		const point_t & p1 = _points [cPoint + 1];

		fLength += ::sqrt ((p1.x - p0.x)*(p1.x - p0.x)*fCosY2 + (p1.y - p0.y)*(p1.y - p0.y));
	}

	return fLength;
}

////////////////////////////////////////////////

float Distance2 (const point_t & _p0, const point_t & _p1, float _fCos2Y) {
	return (_p0.x - _p1.x)*(_p0.x - _p1.x)*_fCos2Y + (_p0.y - _p1.y)*(_p0.y - _p1.y);
}

float DistanceToSegment2 (
	const point_t & _ptA, const point_t & _ptB, const point_t & _pt, float _fCosY,
	point_t & _ptNearest
) {
	const float dx = (_ptB.x - _ptA.x)*_fCosY;
	const float dy = _ptB.y - _ptA.y;

	const float D2 = dx*dx + dy*dy;
	if (D2 < 1e-12) {
		_ptNearest = _ptA;
		return Distance2 (_pt, _ptA, _fCosY*_fCosY);
	}

	const float dxa = (_pt.x - _ptA.x)*_fCosY;
	const float dya = _pt.y - _ptA.y;

	float t = (dxa*dx + dya*dy)/D2;
	if (t < 0)
		t = 0;
	else if (t > 1)
		t = 1;

	_ptNearest.x = _ptA.x + dx*t/_fCosY;
	_ptNearest.y = _ptA.y + dy*t;

	return Distance2 (_pt, _ptNearest, _fCosY*_fCosY);
}

float GetNearestPointOfPolyline (
	const points_t & _points, size_t _cBegin, size_t _cEnd,
	const point_t & _pt,
	float _fCosY,
	point_t & _ptNearest, size_t & _cNearestSegment
) {
	if (_points.empty ())
		return -1;

	if (_cEnd >= _points.size ())
		_cEnd = _points.size ();
	if (_cBegin >= _cEnd)
		return -1;

	float fMinD2 = -1;
	for (size_t c = _cBegin; c + 1 < _cEnd; ++ c) {
		const point_t & ptA = _points [c];
		const point_t & ptB = _points [c + 1];

		point_t ptNearest;
		const float fD2 = DistanceToSegment2 (ptA, ptB, _pt, _fCosY, ptNearest);
		if (fMinD2 > fD2 || fMinD2 < 0) {
			fMinD2           = fD2;
			_ptNearest       = ptNearest;
			_cNearestSegment = c;
		}
	}
	return fMinD2;
}

float Distance2ToPolyline (const points_t & _points, const point_t & _p, float _fCosY) {
	point_t ptNearest;
	size_t cNearestSegment;
	return GetNearestPointOfPolyline (_points, 0, _points.size (), _p, _fCosY, ptNearest, cNearestSegment);
}

float Distance2ToPolygon (const points_t & _points, const point_t & _p, float _fCosY) {
	if (PolygonContainsPt (_points, _p) >= 0)
		return 0;

	const float fD2_1 = Distance2ToPolyline (_points, _p, _fCosY);
	const size_t cPoints = _points.size ();
	if (cPoints <= 2)
		return fD2_1;

	point_t ptNearest;
	const float fD2_2 = DistanceToSegment2 (_points [0], _points [cPoints - 1], _p, _fCosY, ptNearest);
	return __min (fD2_1, fD2_2);
}

///////////////////////////////////////////////////////////

bool AreThereRepeatingPoints (const points_t & _points) {
	const size_t cPoints = _points.size ();
	for (size_t c = 0; c + 1 < cPoints; ++ c) {
		if (_points [c] == _points [c + 1])
			return true;
	}

	return false;
}

void RemoveRepeatingPoints (points_t & _points) {
	for (points_t::iterator i = _points.begin (); i != _points.end (); ++ i) {
		const points_t::iterator i_next = i + 1;
		points_t::iterator i2 = i_next;
		while (i2 != _points.end () && * i == * i2)
			++ i2;
		if (i2 != i_next)
			_points.erase (i_next, i2);
	}
}

////////////////////////////////////////////////

void rect_t::Extend (const points_t & _points) {
	const size_t cPoints = _points.size ();
	for (size_t cPoint = 0; cPoint < cPoints; ++ cPoint)
		Extend (_points [cPoint]);
}

////////////////////////////////////////////////

inline
bool IsInRange (float _x, float _x0, float _x1) {
    return (_x0 <= _x && _x <= _x1) || (_x1 <= _x && _x <= _x0);
}

static
bool RectIntersectsLine (const rect_t & _rect, float _x1, float _y1, float _x2, float _y2) {
	if (_x2 != _x1) {
		const float y1 = (_y2 - _y1)/(_x2 - _x1)*(_rect.x0 - _x1) + _y1;
        if (IsInRange (y1, _rect.y0, _rect.y1) && IsInRange (_rect.x0, _x1, _x2))
            return true;

		const float y2 = (_y2 - _y1)/(_x2 - _x1)*(_rect.x1 - _x1) + _y1;
        if (IsInRange (y2, _rect.y0, _rect.y1) && IsInRange (_rect.x1, _x1, _x2))
            return true;
	}
	if (_y2 != _y1) {
		const float x1 = (_x2 - _x1)/(_y2 - _y1)*(_rect.y0 - _y1) + _x1;
        if (IsInRange (x1, _rect.x0, _rect.x1) && IsInRange (_rect.y0, _y1, _y2))
            return true;

		const float x2 = (_x2 - _x1)/(_y2 - _y1)*(_rect.y1 - _y1) + _x1;
        if (IsInRange (x2, _rect.x0, _rect.x1) && IsInRange (_rect.y1, _y1, _y2))
            return true;
	}
    return _rect.Contains (_x1, _y1);
}

bool RectIntersectsPolyline (const rect_t & _rect, const points_t & _pts, bool _bClosed) {
	if (_pts.empty ())
		return false;

	const size_t cPoints = _pts.size ();
	const size_t cPoints2 = _bClosed ? cPoints : cPoints - 1;
	for (size_t c = 0; c < cPoints2; ++ c) {
		assert (c < cPoints);
		const point_t & a = _pts [c];
		const point_t & b = _pts [c + 1 < cPoints ? c + 1 : 0];

        if (_rect.Contains (a))
            return true;
        if (c == cPoints2 - 1 && _rect.Contains (b))
            return true;
        if (RectIntersectsLine (_rect, a.x, a.y, b.x, b.y))
            return true;
    }
    return false;
}

////////////////////////////////////////////////

bool PointContainsPt (const point_t & _point, const point_t & _pt, float _e, float _fCosY) {
	assert (0 < _fCosY && _fCosY <= 1);
	return ::fabs (_point.x - _pt.x)*_fCosY <= _e && ::fabs (_point.y - _pt.y) <= _e;
}

size_t PointsContainPt (const points_t & _pts, const point_t & _pt, float _e, float _fCosY) {
	if (_pts.empty ())
		return (size_t) -1;
	const size_t cPoints = _pts.size ();
	for (size_t c = 0; c < cPoints; ++ c)
        if (::fabs (_pts [c].x - _pt.x)*_fCosY <= _e && ::fabs (_pts [c].y - _pt.y) <= _e)
            return c;
    return (size_t) -1;
}

//////////////////////////////////

bool SegmentContainsPt (const point_t & _ptA, const point_t & _ptB, const point_t & _pt, float _e, float _fCosY) {
	point_t ptNearest;
	const float fD2 = DistanceToSegment2 (_ptA, _ptB, _pt, _fCosY, ptNearest);
	return fD2 < _e*_e;
}

bool PolylineContainsPt (const points_t & _pts, const point_t & _pt, float _e, float _fCosY, bool _bLastLeg, point_t * _pPoint, size_t * _pcPointIndex) {
	if (_pts.empty ())
		return false;

	assert (0 < _fCosY && _fCosY <= 1);

	const size_t cPoints = _pts.size ();
	const size_t cPoints2 = _bLastLeg ? cPoints : cPoints - 1;
	for (size_t c = 0; c < cPoints2; ++ c) {
		assert (c < cPoints);

		// TODO: ::sqrt() is slow. Use SegmentContainsPt() or DistanceToSegment2() instead of this bulky code!

		const point_t & a = _pts [c];
		const point_t & b = _pts [c + 1 < cPoints ? c + 1 : 0];

		const float dx = (b.x - a.x)*_fCosY;
		const float dy = b.y - a.y;
		const float r = ::sqrt (dx*dx + dy*dy);
		const float fSinA = dy/r;
		const float fCosA = dx/r;
		const float t = (_pt.x - a.x)*_fCosY*fCosA + (_pt.y - a.y)       *fSinA;
		const float d = (_pt.y - a.y)       *fCosA - (_pt.x - a.x)*_fCosY*fSinA;
		if (-_e < d && d < _e && -_e < t && t < r + _e) {
			if (_pPoint) {
				if (t < _e) {
					_pPoint->x = a.x;
					_pPoint->y = a.y;
				} else if (t > r - _e) {
					_pPoint->x = b.x;
					_pPoint->y = b.y;
				} else {
					_pPoint->x = a.x + t*fCosA/_fCosY;
					_pPoint->y = a.y + t*fSinA;
				}
			}
            if (_pcPointIndex) 
                * _pcPointIndex = c;

			return true;  
		}
	}

	return false;
}

bool FrameContainsPt (const rect_t & _rect, const point_t & _pt, float _e, float _fCosY, size_t * _pcPointIndex) {
	points_t points (4);
	points [0] = point_t (_rect.x0, _rect.y0);
	points [1] = point_t (_rect.x0, _rect.y1);
	points [2] = point_t (_rect.x1, _rect.y1);
	points [3] = point_t (_rect.x1, _rect.y0);
	return PolylineContainsPt (points, _pt, _e, _fCosY, true, NULL, _pcPointIndex);
}

///////////////////////////////////////////////////////////////
//
// The algorithm was obtained from pGPSMap.js written by Olexa Ryznik (http://www.olexa.ua)
//
// Returns 1 if point is entirely within polygon,
// 0 if it is on its border and
// -1 if it is outside polygon.
//
int PolygonContainsPt (const points_t & _polygon, const point_t & _p) {
	size_t cc = 0;

	int iLastTouchingLinkSide = 0;
	size_t cFirstPoint = 0;
	bool bFirstPointPassed = false;

	size_t cNodesWithEqualY = 0;

	const size_t cPoints = _polygon.size ();
	for (size_t cPoint = 1; cPoint != cFirstPoint + 1 || ! bFirstPointPassed; ++ cPoint) {
		// Get the first point of leg.
		size_t cPoint0 = cPoint - 1;
		while (cPoint0 >= cPoints)
			cPoint0 -= cPoints;
		const point_t & p0 = _polygon [cPoint0];

		// Get the second point of leg.
		while (cPoint >= cPoints)
			cPoint -= cPoints;
		const point_t & p1 = _polygon [cPoint];

		if (p0 == p1) {
			// Infinite loop protection.
			++ cNodesWithEqualY;
			if (cNodesWithEqualY > cPoints)
				return -1;

			continue;
		}

		if (
			(p0.y < _p.y && _p.y < p1.y) ||
			(p0.y > _p.y && _p.y > p1.y)
		) {
			// Leg crosses point's latitude.
			const float x = p0.x + (_p.y - p0.y)*(p1.x - p0.x)/(p1.y - p0.y);
			if (x == _p.x)
				// Leg crosses the point.
				return 0;
			else if (x < _p.x)
				// Leg passes under the point.
				++ cc;

			bFirstPointPassed = true;
		} else if (p0.y == _p.y && p1.y == _p.y) {
			// Leg is entirely within point's latitude.
			if (
				(p0.x <= _p.x && p1.x >= _p.x) ||
				(p0.x >= _p.x && p1.x <= _p.x)
			)
				// Leg crosses the point.
				return 0;

			if (cFirstPoint == cPoint - 1 && p1.x < _p.x)
				// There was no any link that crosses point's latitude or finishes at it yet.
				++ cFirstPoint;

			// Infinite loop protection.
			assert (p0.y == p1.y);
			++ cNodesWithEqualY;
			if (cNodesWithEqualY > cPoints)
				return -1;
		} else if (p0.y != _p.y && p1.y == _p.y) {
			// Leg finishes at point's latitude.
			if (p1.x == _p.x)
				// Leg crosses the point.
				return 0;
			else if (p1.x < _p.x)
				// Remember last touching leg side.
				iLastTouchingLinkSide = p0.y < _p.y ? -1 : 1;
			bFirstPointPassed = true;
		} else if (p0.y == _p.y && p1.y != _p.y) {
			// Leg starts at point's latitude.
			if (p0.x == _p.x)
				// Leg crosses the point.
				return 0;
			else if (p0.x < _p.x)
				if (iLastTouchingLinkSide == 0)
					// There was no touching leg yet.
					// We should loop through the polygon 'till this point.
					cFirstPoint = cPoint;
				else if (
					iLastTouchingLinkSide == -1 && p1.y > _p.y ||
					iLastTouchingLinkSide == 1  && p1.y < _p.y
				)
					// This links with previous touching leg together cross point's latitude.
					++ cc;
		} else
			// Leg does not cross point's latitude.
			bFirstPointPassed = true;
	}
	return (cc & 0x1) ? 1 : -1;
}

/////////////////////////////////////////////

static
bool _LegsAreIntersecting (
	const point_t & _a, const point_t & _b,
	const point_t & _c, const point_t & _d
) {
	const double c_fE = 1e-10f;

	const double t = double (_d.x - _c.x)*(_b.y - _a.y) - double (_d.y - _c.y)*(_b.x - _a.x);
	if (::fabs (t) < c_fE)
		return false;

	const double p = (double (_c.y - _a.y)*(_b.x - _a.x) - double (_c.x - _a.x)*(_b.y - _a.y))/t;
	if (p <= c_fE || p >= 1 - c_fE)
		return false;

	const double q = (double (_c.y - _a.y)*(_d.x - _c.x) - double (_c.x - _a.x)*(_d.y - _c.y))/t;
	if (q <= c_fE || q >= 1 - c_fE)
		return false;

	return true;
}

bool SegmentIntersectsPolyline (const point_t & _a, const point_t & _b, const points_t & _points, bool _bClosed) {
	const size_t cPoints = _points.size ();
	const size_t cPoints_e = _bClosed ? cPoints : cPoints - 1;

	for (size_t n = 0; n < cPoints_e; ++ n) {
		const point_t & c = _points [n];
		const point_t & d = _points [n + 1 < cPoints ? n + 1 : 0];

		if (_LegsAreIntersecting (_a, _b, c, d))
			return true;
	}

	return false;
}

bool PolylineIntersectsPolyline (const points_t & _points1, const points_t & _points2, bool _bClosed, pip_info_t * _pInfo) {
	const size_t cPoints1 = _points1.size ();
	const size_t cPoints1e = _bClosed ? cPoints1 : cPoints1 - 1;

	const size_t cPoints2 = _points2.size ();
	const size_t cPoints2e = _bClosed ? cPoints2 : cPoints2 - 1;

	for (size_t c1 = 0; c1 < cPoints1e; ++ c1) {
		const point_t & a = _points1 [c1];
		const point_t & b = _points1 [c1 + 1 < cPoints1 ? c1 + 1 : 0];

		for (size_t c2 = 0; c2 < cPoints2e; ++ c2) {
			const point_t & c = _points2 [c2];
			const point_t & d = _points2 [c2 + 1 < cPoints2 ? c2 + 1 : 0];

			if (_LegsAreIntersecting (a, b, c, d)) {
				if (_pInfo) {
					_pInfo->cSegment1 = c1;
					_pInfo->cSegment2 = c2;
				}
				return true;
			}
		}
	}

	return false;
}

bool PolylineIntersectsSelf (const points_t & _points, bool _bClosed, pip_info_t * _pInfo) {
	return PolylineIntersectsPolyline (_points, _points, _bClosed, _pInfo);
}

bool PolylineIntersectsPolylineInNode (const points_t & _points1, const points_t & _points2, pip_info_t * _pInfo) {
	const size_t cPoints1 = _points1.size ();
	const size_t cPoints2 = _points2.size ();
	for (size_t c1 = 0; c1 < cPoints1; ++ c1) {
		const point_t & pt1 = _points1 [c1];

		for (size_t c2 = 0; c2 < cPoints2; ++ c2) {
			const point_t & pt2 = _points2 [c2];

			if (pt1 == pt2) {
				if (_pInfo) {
					_pInfo->cSegment1 = c1;
					_pInfo->cSegment2 = c2;
				}
				return true;
			}
		}
	}
	return false;
}

bool PolylineIntersectsSelfInNode (const points_t & _points, pip_info_t * _pInfo) {
	const size_t cPoints = _points.size ();
	for (size_t c1 = 0; c1 < cPoints; ++ c1) {
		const point_t & pt1 = _points [c1];

		for (size_t c2 = c1 + 1; c2 < cPoints; ++ c2) {
			const point_t & pt2 = _points [c2];

			if (pt1 == pt2) {
				if (_pInfo) {
					_pInfo->cSegment1 = c1;
					_pInfo->cSegment2 = c2;
				}
				return true;
			}
		}
	}

	return false;
}

///////////////////////////////////////////////////////

void RemoveRepeatingPoints (epoints_t & _epoints) {
	// Check for fully equal points/extras.
	epoints_t::iterator i;
	for (i = _epoints.begin (); i != _epoints.end (); ++ i) {
		bool bRemoveI;

		do {
			const bool bEmpty = i->extra.empty ();
			bRemoveI = false;

			const epoints_t::iterator i_next = i + 1;
			epoints_t::iterator i2 = i_next;
			while (i2 != _epoints.end () && i->point == i2->point) {
				const bool bEmpty2 = i2->extra.empty ();
				if (! bEmpty && ! bEmpty2 && ! (i->extra == i2->extra))
					break;
				if (bEmpty && ! bEmpty2) {
					bRemoveI = true;
					break;
				}
				++ i2;
			}
			if (i2 != i_next)
				_epoints.erase (i_next, i2);

			if (bRemoveI) {
				i = _epoints.erase (i);
				assert (i != _epoints.end ());
			}
		} while (bRemoveI);
	}

	// Check for repeating points with mutually adjacent house numbers.
	for (i = _epoints.begin (); i != _epoints.end (); ++ i) {
		const numbers_t & Numbers = i->extra.Numbers;
		if (! Numbers.bHasNumbers)
			continue;

		const epoints_t::iterator i_next = i + 1;
		if (i_next == _epoints.end ())
			continue;
		if (! (i->point == i_next->point))
			continue;

		numbers_t & NumbersNext = i_next->extra.Numbers;
		if (! NumbersNext.bHasNumbers)
			continue;

		// Join the 2 nodes.
		NumbersNext.wLeftBefore  = Numbers.wLeftBefore;
		NumbersNext.wRightBefore = Numbers.wRightBefore;

		i = _epoints.erase (i, i_next);
	}
}

void Points2EPoints (const points_t & _points, const point_extras_t * _pPointExtras, epoints_t & _epoints) {
	const size_t cPoints = _points.size ();
	_epoints.resize (cPoints);

	for (size_t c = 0; c < cPoints; ++ c) {
		epoint_t & ep = _epoints [c];
		ep.point = _points [c];
		ep.extra = _pPointExtras && c < _pPointExtras->size () ? (* _pPointExtras) [c] : point_extra_t ();
	}
}

void EPoints2Points (const epoints_t & _epoints, bool _bHasExtra, points_t & _points, point_extras_t & _PointExtras) {
	const size_t cPoints = _epoints.size ();

	_points.resize (cPoints);
	for (size_t c = 0; c < cPoints; ++ c)
		_points [c] = _epoints [c].point;

	if (_bHasExtra) {
		_PointExtras.resize (cPoints);
		for (size_t c = 0; c < cPoints; ++ c)
			_PointExtras [c] = _epoints [c].extra;
	}
}

